#ifndef _MODEL_
#define _MODEL_

#include <vector>
#include "BodyLink.h"
#include "Dof.h"
#include "Transformation.h"
#include "Marker.h"
#include "Joint.h"

class Model{
protected:
	BodyLink *mRoot;
	vector<Marker *> mHandles;
	vector<Dof *> mDofs;
	vector<Transformation *> mTransforms;
	vector<BodyLink *> mBodyLinks;
	vector<Joint *> mJoints;

	void *mModelData;	// aux pointer to hold any extra data

public:
	Model();
	~Model(){
		for(int i=0; i<mJoints.size(); i++) delete mJoints[i];
		mJoints.clear();
		mDofs.clear();
		mTransforms.clear();
		for(int i=0; i<mBodyLinks.size(); i++) delete mBodyLinks[i];
		mBodyLinks.clear();
		mHandles.clear();
		if(mTransformMap!=NULL){
			delete[] mTransformMap;
			mTransformMap=NULL;
		}
	}
	void switchBetweenLastAndCurrentDof();
	void addHandle(Marker *h);
	void addBodyLink(BodyLink *b);
	void addJoint(Joint *_j);
	void addDof(Dof *d);
	void addTransform(Transformation *t);
	Vecd getQ();
	Vecd getQDot();
	// init the model after parsing
	void initModel();
	void setDependDofMap(BodyLink *b);
	double getMass();
	Vec3d computeCOM();

	// just to tag each side with a name
	enum BodySide {LEFT, RIGHT, NONE, BOTH};

	// useful pointers;
	BodyLink * mFootLeft;
	BodyLink * mFootRight;
	BodyLink * mToeLeft;
	BodyLink * mToeRight;
	BodyLink *mSpine;
	BodyLink *mHandLeft;
	BodyLink *mHandRight;
	BodyLink *mUpperarmLeft;
	BodyLink *mUpperarmRight;
	int* mTransformMap;

	// inline access functions
	inline int getNumDofs(){return static_cast<int>(mDofs.size());}
	inline int getNumTransforms(){return static_cast<int>(mTransforms.size());}
	inline int getNumBodyLinks(){return static_cast<int>(mBodyLinks.size());}
	inline int getNumHandles(){ return static_cast<int>(mHandles.size());}
	inline Dof* getDof(int i){return mDofs[i];}
	inline Transformation* getTransform(int i){return mTransforms[i];}
	inline BodyLink* getBodyLink(int i){return mBodyLinks[i];}
	inline BodyLink* getRoot(){return mRoot;}
	inline Marker*   getHandle(int i){ return mHandles[i];}

	inline void draw( Vec4d _color=Vec4d(vl_1), bool _default=true ){mRoot->draw(_color, _default);}
	inline void drawHandles( Vec4d _color=Vec4d(vl_1), bool _default=true ){mRoot->drawHandles(_color, _default);}
	inline void setPose(vector<double> *_pose){
		for(int i=0; i<mDofs.size(); i++) mDofs[i]->setValue(_pose->at(i));
	}
	inline void getPose(vector<double> *_pose){
		_pose->clear();
		for(int i=0; i<mDofs.size(); i++) _pose->push_back(mDofs[i]->getValue());
	}

	inline void setModelData(void *_md){mModelData=_md;}
	inline void*getModelData(){return mModelData;}
};

#endif
